#ifndef EVENT_PROCESS_NODE_H_
#define EVENT_PROCESS_NODE_H_

#include <queue>
#include <vector>
#include <thread>
#include <mutex>

#include <ros/ros.h>
#include <ros/node_handle.h>

#include <std_msgs/Time.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <message_filters/cache.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/MatrixFunctions>
#include <image_transport/image_transport.h>

#include <dvs_msgs/Event.h>
#include <dvs_msgs/EventArray.h>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "motion_compensation/bias.h" // 自定义消息

using cv::Mat;
using cv::Mat_;
using namespace std;

const int sensor_rows = 180;
const int sensor_cols = 240;

// const int sensor_rows = 260;
// const int sensor_cols = 346;

class EventProcessNode
{
public:
	EventProcessNode(ros::NodeHandle &nh);
	~EventProcessNode();

	void eventCallback(const dvs_msgs::EventArray::ConstPtr &emsg); // 事件回调
	void imageCallback(const sensor_msgs::ImageConstPtr &img_msg);	// 图像回调
	void imuCallback(const sensor_msgs::Imu::ConstPtr &imu_msg);	// imu回调函数(什么也不做)
	void baCallback(const motion_compensation::biasPtr &msg);		// ba
	void bgCallback(const motion_compensation::biasPtr &msg);		// bg

	std::tuple<Eigen::Vector3f, Eigen::Vector3f> imuIntegrate(std::vector<sensor_msgs::Imu::ConstPtr> &imu_vector) const; // 预积分得到位姿

	void motion_compensate();									 // 运动补偿
	void no_motion_compensate();								 // 没有运动补偿
	void clearEventQueue();										 // 清除事件队列
	inline bool IsWithinTheBoundary(const int &x, const int &y); // 检测像素范围

	void unproject(Eigen::Vector3f &vec); // 反投影(没有用到)
	void project(Eigen::Vector3f &vec);	  // 投影(没有用到)

private:
	ros::NodeHandle &nh_;									// 句柄
	image_transport::ImageTransport it_;					// 图像
	ros::Subscriber events_sub_;							// 事件订阅
	ros::Subscriber img_sub_;								// 图像订阅
	ros::Subscriber ba_sub_, bg_sub_;						// 偏置订阅
	message_filters::Subscriber<sensor_msgs::Imu> imu_sub_; // imu订阅
	image_transport::Publisher event_image_pub_;			// 发布事件图像
	image_transport::Publisher raw_image_pub_;				// 发布原始图像
	cv::Mat EventAccumulatedImage_;							// 事件图像

	std::unique_ptr<message_filters::Cache<sensor_msgs::Imu>> imu_cache_; // imu缓存, 用于找到预积分起点终点
	vector<sensor_msgs::Imu::ConstPtr> imu_buffer_;						  // imu队列
	vector<dvs_msgs::Event> events_buffer_;								  // 事件队列

	int window_size_;		// 累积的事件数量
	float median_depth_;	// 场景平均深度, 反投影时用到
	int event_size_;		// 事件队列长度
	std::mutex data_mutex_; // 处理事件时加锁
	ros::Time t0, t1, tt;	// 预积分, 线性插值相关时刻
	double deltaT;			// tt - t0
	bool first_event;		// 初始化事件队列,定义t0

	Eigen::Vector3f b_w_; // 陀螺仪偏置
	Eigen::Vector3f b_a_; // 加速度计偏置

	Eigen::Vector3f R10; // 预积分得到的平移
	Eigen::Vector3f t10; // 预积分得到的旋转

	Eigen::Matrix3f K;		   // 投影矩阵
	Eigen::Matrix3f K_inverse; // 投影矩阵的逆

	Eigen::Vector3f re;		  // 单个事件的旋转向量
	Eigen::Vector3f te;		  // 单个事件的平移向量
	Eigen::Matrix3f rot;	  // 单个事件的旋转矩阵
	Eigen::Vector3f eventVec; // 单个事件的齐次坐标

	int map_blur_;

	ros::Time image_time; // 第一帧图像的时间戳
};

#endif
